#ifndef __NAVIGATION_HPP_
#define __NAVIGATION_HPP_
///////////////////////////////////
#include <iostream>
#include <vector>
#include <algorithm>
#include "../base.hpp"

typedef struct DijkstraNode
{
    Point2D position; // 节点坐标
    int distance;     // 节点离起点的距离
    int prevIndex;    // 最短路径中的上一个节点

    /// @brief 重载'<'运算符
    bool operator<(const DijkstraNode &other) const
    {
        return distance < other.distance;
    }
} DijkstraNode;

/// @brief 导航类，采用Dijkstra算法
class Navigator
{
public:
    vector<DijkstraNode> nodes;
    vector<DijkstraNode> updatedNodes;

    Navigator()
    {
    }

    /// @brief 初始化导航器，如果要推箱子，boxes应当输入空
    void init(vector<vector<bool>> map, vector<Point2D> boxes)
    {
        nodes.clear();
        updatedNodes.clear();
        // 把所有节点放入节点列表
        for (int i = 0; i < map.size(); i++)
            for (int j = 0; j < map[0].size(); j++)
                if (!map[i][j])
                {
                    bool flag = false;
                    for (Point2D box : boxes)
                        if (box.x == j && box.y == i)
                            flag = true;
                    if (!flag)
                        nodes.push_back({{j, i}, 0, -1});
                }
    }

    /// @brief 寻找并返回路径
    /// @param from 起点坐标
    /// @param to 终点坐标
    /// @return 得出的路径
    vector<Point2D> navigate(Point2D from, Point2D to)
    {
        // 特殊情况
        if (from == to)
            return vector<Point2D>();
        // 检查输入
        bool flag1 = false, flag2 = false;
        for (DijkstraNode node : nodes)
        {
            if (node.position == from)
                flag1 = true;
            if (node.position == to)
                flag2 = true;
        }
        if (!(flag1 && flag2))
        {
            cerr << "Cannot move!!" << endl;
            return vector<Point2D>();
        }

        // 开始
        int n = 0, index = -1;
        int dx, dy;
        for (int i = 0; i < nodes.size(); i++)
            if (nodes[i].position == from)
                index = i;

        updatedNodes.push_back(nodes[index]);
        nodes.erase(nodes.begin() + index);
        while (nodes.size() > 0)
        {
            /// 寻找和已有节点相邻的节点
            int choiceIndex = -1;
            for (int i = 0; i < nodes.size(); i++)
            {
                for (int j = 0; j < updatedNodes.size(); j++)
                {
                    int dx = nodes[i].position.x - updatedNodes[j].position.x;
                    int dy = nodes[i].position.y - updatedNodes[j].position.y;
                    if (abs(dx) + abs(dy) > 1)
                        continue;
                    choiceIndex = i;
                    break;
                }
                if (choiceIndex > 0)
                    break;
            }
            if (choiceIndex == -1)
                break;
            // 取出一个节点
            updatedNodes.push_back(nodes[choiceIndex]);
            nodes.erase(nodes.begin() + choiceIndex);
            n++;
            /// 计算新节点的距离
            int index = -1, mindist = 114514;
            for (int i = 0; i < updatedNodes.size() - 1; i++)
            { // 寻找相邻节点
                dx = updatedNodes[i].position.x - updatedNodes[n].position.x;
                dy = updatedNodes[i].position.y - updatedNodes[n].position.y;
                if (abs(dx) + abs(dy) != 1)
                    continue;
                if (updatedNodes[i].distance + 1 < mindist)
                { // 选择最邻近且下标最近的节点
                    mindist = updatedNodes[i].distance + 1;
                    index = i;
                }
            }
            updatedNodes[n].distance = mindist;
            updatedNodes[n].prevIndex = index;

            /// 更新节点列表
            for (int i = 0; i < updatedNodes.size() - 1; i++)
            { // 寻找相邻节点
                dx = updatedNodes[i].position.x - updatedNodes[n].position.x;
                dy = updatedNodes[i].position.y - updatedNodes[n].position.y;
                if (abs(dx) + abs(dy) != 1)
                    continue;
                if (mindist + 1 < updatedNodes[i].distance)
                {
                    updatedNodes[i].distance = mindist + 1;
                    updatedNodes[i].prevIndex = n;
                }
            }
        }

        index = -1;
        for (int i = 0; i < updatedNodes.size(); i++)
        {
            DijkstraNode node = updatedNodes[i];
            if (node.position == to)
            {
                index = i;
            }
        }

        if (index == -1)
        {
            cerr << "Cannot move!!" << endl;
            return vector<Point2D>();
        }
        // for (DijkstraNode node : updatedNodes)
        // {
        //     cout << node.position.x << " " << node.position.y << " " << node.distance << ' ';
        //     cout << "pindex=" << node.prevIndex << endl;
        // }

        vector<Point2D> path;
        DijkstraNode node = updatedNodes[index];
        while (node.prevIndex >= 0)
        {
            path.push_back(node.position);
            node = updatedNodes[node.prevIndex];
        }
        path.push_back(from);
        reverse(path.begin(), path.end());
        return path;
    }

    // 把路径转化成 WASD 指令
    string path2cmd(vector<Point2D> path)
    {
        string cmd="";
        int dx, dy;
        for (int i = 1; i < path.size(); i++)
        {
            dx = path[i].x - path[i - 1].x;
            dy = path[i].y - path[i - 1].y;
            cmd += dxy2wasd(dx, dy);
        }
        return cmd;
    }

    int dxy2wasd(int dx, int dy)
    {
        if (dx == 1 && dy == 0)
            return 'D';
        if (dx == -1 && dy == 0)
            return 'A';
        if (dx == 0 && dy == 1)
            return 'S';
        if (dx == 0 && dy == -1)
            return 'W';
        cerr << "wrong input at dxy2wasd" << endl;
        return 'E';
    }
};

////////////////////////////////////
#endif